#ifndef MOTION_KALMANFILTER_H
#define MOTION_KALMANFILTER_H

#include "postprocess/kalman_filter/base_kalmanfilter.h"
#include "postprocess/common/common_var.h"

// 这个滤波器用于更新x y vx vy ax ay
// 状态量为 x y vx vy ax ay
// 观测量为 x y vx vy
// dynam_param_num=6, int measure_param_num=6,control_param_num = 0
class MotionKalmanFilter : public BaseKalmanFilter {
 public:
  MotionKalmanFilter();
  MotionKalmanFilter(const ProcessorClass& flag);
  MotionKalmanFilter(MotionKalmanFilter const&) = delete;
  MotionKalmanFilter& operator=(MotionKalmanFilter const&) = delete;
  MotionKalmanFilter(MotionKalmanFilter&&) = delete;
  MotionKalmanFilter& operator=(MotionKalmanFilter&&) = delete;
  ~MotionKalmanFilter();

  /// @brief init function
  void Init(int dynam_param_num, int measure_param_num,
            int control_param_num = 0) override;

  /// @brief update filter with object
  /// @param new_object
  /// @param old_object
  /// @param time_diff
  /// @param is_utm_flag
  void UpdateWithObject(TrackedObjectPtr& new_object,
                        TrackedObjectPtr const& old_object,
                        double time_diff) override;

  /// @brief set Q
  /// @param Matrix Q
  void SetQ(Eigen::MatrixXf& Q) override;

  /// @brief set R
  /// @param Matrix R
  void SetR(Eigen::MatrixXf& R) override;

  /// @brief get state of filter
  /// @param center_point
  /// @param velocity
  /// @param acceleration
  void GetState(Eigen::Vector3f* center_point, float& vx, float& vy, float& ax,
                float& ay) noexcept;

  /// @brief get state of filter
  /// @param anchor_point
  /// @param velocity
  /// @param acceleration
  void GetState(Eigen::Vector3f* center_point, Eigen::Vector3f* velocity,
                Eigen::Vector3f* acceleration) noexcept;

  /// @brief update filter without object
  /// @param time_diff
  void UpdateWithoutObject(double time_diff) override;

  /// @brief set initial position
  /// @param velocity to be set
  /// @return void
  void SetCenterPoint(const Eigen::Vector3f& point) noexcept;

  /// @brief set initial velocity
  /// @param velocity to be set
  /// @return void
  void SetVelocity(const Eigen::Vector3f& vel) noexcept;

  /// @brief set initial acceleration
  /// @param velocity to be set
  /// @return void
  void SetAcceleration(const Eigen::Vector3f& acc) noexcept;

  /// @brief set post settings
  /// @param PostProcessorSettings to be set
  /// @return void
  void SetSettings(PostProcessorSettings const& settings) noexcept;

  /// @brief get position
  /// @param velocity to be get
  /// @return void
  void GetCenterPoint(Eigen::Vector3f& point) noexcept;

  /// @brief get velocity
  /// @param velocity to be get
  /// @return void
  void GetVelocity(Eigen::Vector3f& vel) noexcept;

  /// @brief get acceleration
  /// @param velocity to be get
  /// @return void
  void GetAcceleration(Eigen::Vector3f& acc) noexcept;

  /// @brief get acceleration
  /// @param velocity to be get
  /// @return void
  void GetProcessorClassFlag(ProcessorClass& flag) noexcept;

  /// @brief get acceleration
  /// @param velocity to be get
  /// @return void
  void SetProcessorClassFlag(ProcessorClass flag) noexcept;

 protected:
  /// @brief kalman filter update state with  observation
  /// @param
  void update_state(TrackedObjectPtr const& new_object,
                    TrackedObjectPtr const& old_object,
                    double time_diff) override;

  /// @brief adjust kalman gain
  /// @param
  void state_gain_adjustment(Eigen::VectorXf& gain,
                             double time_diff = 0.0) override;

 private:
  /// @brief center point
  Eigen::Vector3f belief_center_point_;
  /// @brief belief velocity
  Eigen::Vector3f belief_velocity_;
  /// @brief belief acceleration
  Eigen::Vector3f belief_acceleration_;
  /// @brief proseccor flag
  ProcessorClass processor_flag_;
  /// @brief proseccor settings
  PostProcessorSettings settings_;
};

#endif
